Using ROS 2? Check out the ROS 2 tf2 tutorials.
| Note: This tutorial assumes you have a working knowledge of compiling programs in ROS and you have completed the Introduction to tf2 tutorial. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Writing a tf2 broadcaster (Python)
Description: This tutorial teaches you how to broadcast the state of a robot to tf2.Tutorial Level: BEGINNER
Next Tutorial: Writing a tf2 listener (Python)
Contents
In the next two tutorials we will write the code to reproduce the demo from the tf2 introduction tutorial. After that, the following tutorials focus on extending the demo with more advanced tf2 features.
Create a learning_tf2 package
First we will create a catkin package that will be used for this tutorial and the following ones. This package called learning_tf2 will depend on tf2, tf2_ros, roscpp, rospy and turtlesim. Code for this tutorial is stored here.
$ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim
How to broadcast transforms
This tutorial teaches you how to broadcast coordinate frames to tf2. In this case, we want to broadcast the changing coordinate frames of the turtles, as they move around.
Let's first create the source files. Go to the package we just created:
$ roscd learning_tf2
The Code
Let's start by making a new directory called nodes in our learning_tf2 package.
$ mkdir nodes
Fire up your favorite editor and paste the following code into a new file called nodes/turtle_tf2_broadcaster.py.
1 #!/usr/bin/env python
2 import rospy
3
4 # Because of transformations
5 import tf_conversions
6
7 import tf2_ros
8 import geometry_msgs.msg
9 import turtlesim.msg
10
11
12 def handle_turtle_pose(msg, turtlename):
13 br = tf2_ros.TransformBroadcaster()
14 t = geometry_msgs.msg.TransformStamped()
15
16 t.header.stamp = rospy.Time.now()
17 t.header.frame_id = "world"
18 t.child_frame_id = turtlename
19 t.transform.translation.x = msg.x
20 t.transform.translation.y = msg.y
21 t.transform.translation.z = 0.0
22 q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
23 t.transform.rotation.x = q[0]
24 t.transform.rotation.y = q[1]
25 t.transform.rotation.z = q[2]
26 t.transform.rotation.w = q[3]
27
28 br.sendTransform(t)
29
30 if __name__ == '__main__':
31 rospy.init_node('tf2_turtle_broadcaster')
32 turtlename = rospy.get_param('~turtle')
33 rospy.Subscriber('/%s/pose' % turtlename,
34 turtlesim.msg.Pose,
35 handle_turtle_pose,
36 turtlename)
37 rospy.spin()
Don't forget to make the node executable:
chmod +x nodes/turtle_tf2_broadcaster.py
The Code Explained
Now, let's take a look at the code that is relevant to publishing the turtle pose to tf2.
32 turtlename = rospy.get_param('~turtle')
This node takes a single parameter "turtle", which specifies a turtle name, e.g. "turtle1" or "turtle2".
33 rospy.Subscriber('/%s/pose' % turtlename,
The node subscribes to topic "turtleX/pose" and runs function handle_turtle_pose on every incoming message.
Now, we create a Transform object and give it the appropriate metadata.
We need to give the transform being published a timestamp, we'll just stamp it with the current time, ros::Time::now().
Then, we need to set the name of the parent frame of the link we're creating, in this case "world"
- Finally, we need to set the name of the child node of the link we're creating, in this case this is the name of the turtle itself.
The handler function for the turtle pose message broadcasts this turtle's translation and rotation, and publishes it as a transform from frame "world" to frame "turtleX".
19 t.transform.translation.x = msg.x
20 t.transform.translation.y = msg.y
21 t.transform.translation.z = 0.0
22 q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
23 t.transform.rotation.x = q[0]
24 t.transform.rotation.y = q[1]
25 t.transform.rotation.z = q[2]
26 t.transform.rotation.w = q[3]
Here we copy the information from the 3D turtle pose into the 3D transform.
28 br.sendTransform(t)
This is where the real work is done. Sending a transform with a TransformBroadcaster requires passing in just the transform itself.
Note: you can also publish static transforms on the same pattern by instantiating a tf2_ros.StaticTransformBroadcaster instead of a tf2_ros.TransformBroadcaster. The static transforms will be published on the /tf_static topic and will be sent only when required(latched topic) and not periodically. For more details see here
Running the broadcaster
Now create a launch file for this demo. With your text editor, create a new file called start_demo.launch, and add the following lines:
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<node name="turtle1_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle1" />
</node>
<node name="turtle2_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
<param name="turtle" type="string" value="turtle2" />
</node>
</launch>
Build your scripts
This tutorial does not include the install rules necessary if you were to want to release this.
In your workspace invoke catkin_make
And source devel_isolated/setup.bash in your terminal.
Now you're ready to start your own turtle broadcaster demo:
$ roslaunch learning_tf2 start_demo.launch
You should see the turtle sim with one turtle.
Checking the results
Now, use the tf_echo tool to check if the turtle pose is actually getting broadcast to tf2:
$ rosrun tf tf_echo /world /turtle1
This should show you the pose of the first turtle. Drive around the turtle using the arrow keys (make sure your terminal window is active, not your simulator window). If you run tf_echo for the transform between the world and turtle 2, you should not see a transform, because the second turtle is not there yet. However, as soon as we add the second turtle in the next tutorial, the pose of turtle 2 will be broadcast to tf2.
To actually use the transforms broadcast to tf2, you should move on to the next tutorial about creating a tf2 listener (Python) (C++)
